/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

#include "RallyPointManager.h"
#include "Vehicle.h"
#include "MAVLinkProtocol.h"
#include "QGCApplication.h"

QGC_LOGGING_CATEGORY(RallyPointManagerLog, "RallyPointManagerLog")

RallyPointManager::RallyPointManager(Vehicle* vehicle)
    : QObject(vehicle)
    , _vehicle(vehicle)
{
    _planType =MAV_MISSION_TYPE_RALLY;
}

RallyPointManager::~RallyPointManager()
{

}

void RallyPointManager::_sendError(ErrorCode_t errorCode, const QString& errorMsg)
{
    qCDebug(RallyPointManagerLog) << "Sending error" << errorCode << errorMsg;

    emit error(errorCode, errorMsg);
}

void RallyPointManager::loadFromVehicle(void)
{
    // No support in generic vehicle
    emit loadComplete(QList<QGeoCoordinate>());
}
void RallyPointManager::_handleMissionRequest(const mavlink_message_t& message, bool missionItemInt)
{

}
void RallyPointManager::_mavlinkMessageReceived(const mavlink_message_t& message)
{
    switch (message.msgid) {
    case MAVLINK_MSG_ID_MISSION_COUNT:
       // _handleMissionCount(message);
        break;

    case MAVLINK_MSG_ID_MISSION_ITEM:
       // _handleMissionItem(message, false /* missionItemInt */);
        break;

    case MAVLINK_MSG_ID_MISSION_ITEM_INT:
       // _handleMissionItem(message, true /* missionItemInt */);
        break;

    case MAVLINK_MSG_ID_MISSION_REQUEST:
        _handleMissionRequest(message, false /* missionItemInt */);
        break;

    case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
        _handleMissionRequest(message, true /* missionItemInt */);
        break;

    case MAVLINK_MSG_ID_MISSION_ACK:
        //_handleMissionAck(message);
        break;
    }
}
void RallyPointManager::_connectToMavlink(void)
{
    connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &RallyPointManager::_mavlinkMessageReceived);
}

void RallyPointManager::_disconnectFromMavlink(void)
{
    disconnect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &RallyPointManager::_mavlinkMessageReceived);
}

void RallyPointManager::sendToVehicle(const QList<QGeoCoordinate>& rgPoints)
{
    // No support in generic vehicle
    //Q_UNUSED(rgPoints);
    mavlink_message_t message;

    _dedicatedLink = _vehicle->priorityLink();
    _connectToMavlink();
    mavlink_msg_mission_count_pack_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
                                        qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
                                        _dedicatedLink->mavlinkChannel(),
                                        &message,
                                        _vehicle->id(),
                                        MAV_COMP_ID_MISSIONPLANNER,
                                        rgPoints.count(),
                                        _planType);

    _vehicle->sendMessageOnLink(_dedicatedLink, message);

    //emit sendComplete(false /* error */);
}

void RallyPointManager::removeAll(void)
{
    // No support in generic vehicle
    emit removeAllComplete(false /* error */);
}

bool RallyPointManager::supported(void) const
{
    return (_vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY) && (_vehicle->maxProtoVersion() >= 200);
}
